#include "cw_cmdslove.h"

static int msgctr=0;
char dbgstxt[20];
void trackcmd_process(void)
{
	if(GET_CTRLFLAG()&TCKCTRL)
	{
		Track.ready=false;
		if(TCK_RxProBuf[0]=='C')
		{
			uint8_t* tempheader = &TCK_RxProBuf[1];

			if(*tempheader=='E')
			{
				Track.err=FloatNum_analysis(tempheader+sizeof(uint8_t), ',',&tempheader);
			}
			if(*tempheader=='M')
			{
				Track.mode=Numerical_analysis(tempheader+sizeof(uint8_t),',',&tempheader);
			}
			if(*tempheader=='S')
			{
				Track.exceptspeed=Numerical_analysis(tempheader+sizeof(uint8_t),';',&tempheader);
			}
		}
		else
		{
			trackUartSend(TCK_RxProBuf, sizeof(CTL_RxProBuf)-1);
		}
		sprintf(dbgstxt,"%f",Track.err);
		debugUartSend((uint8_t*)dbgstxt, sizeof(dbgstxt)-1);
		Track.ready=true;
		msgctr++;
		STALED_STA(msgctr);
	}
	else
	{
		Track.err=0;
		Track.mode=0;
		Track.exceptspeed=0;
	}
}

void DBG_PID_Process(void)
{
	//左轮速度PID（增量）
	if(DBG_RxProBuf[3]=='L'&&DBG_RxProBuf[4]=='V')
	{
		uint8_t* tempheader = &DBG_RxProBuf[5];
		if(*tempheader=='P')
		{
			LspdPID.Kp=FloatNum_analysis(tempheader+sizeof(uint8_t),',',&tempheader);
		}
		if(*tempheader=='I')
		{
			LspdPID.Ki=FloatNum_analysis(tempheader+sizeof(uint8_t),',',&tempheader);
		}
		if(*tempheader=='D')
		{
			LspdPID.Kd=FloatNum_analysis(tempheader+sizeof(uint8_t),';',&tempheader);
		}
	}
	//右轮速度PID（增量）
	else if(DBG_RxProBuf[3]=='R'&&DBG_RxProBuf[4]=='V')
	{
		uint8_t* tempheader = &DBG_RxProBuf[5];
		if(*tempheader=='P')
		{
			RspdPID.Kp=FloatNum_analysis(tempheader+sizeof(uint8_t),',',&tempheader);
		}
		if(*tempheader=='I')
		{
			RspdPID.Ki=FloatNum_analysis(tempheader+sizeof(uint8_t),',',&tempheader);
		}
		if(*tempheader=='D')
		{
			RspdPID.Kd=FloatNum_analysis(tempheader+sizeof(uint8_t),';',&tempheader);
		}
	}
	else if(DBG_RxProBuf[3]=='L'&&DBG_RxProBuf[4]=='P')
	{
		uint8_t* tempheader = &DBG_RxProBuf[5];
		if(*tempheader=='P')
		{
			LposPID.Kp=FloatNum_analysis(tempheader+sizeof(uint8_t),',',&tempheader);
		}
		if(*tempheader=='I')
		{
			LposPID.Ki=FloatNum_analysis(tempheader+sizeof(uint8_t),',',&tempheader);
		}
		if(*tempheader=='D')
		{
			LposPID.Kd=FloatNum_analysis(tempheader+sizeof(uint8_t),';',&tempheader);
		}
	}
	else if(DBG_RxProBuf[3]=='R'&&DBG_RxProBuf[4]=='P')
	{
		uint8_t* tempheader = &DBG_RxProBuf[5];
		if(*tempheader=='P')
		{
			RposPID.Kp=FloatNum_analysis(tempheader+sizeof(uint8_t),',',&tempheader);
		}
		if(*tempheader=='I')
		{
			RposPID.Ki=FloatNum_analysis(tempheader+sizeof(uint8_t),',',&tempheader);
		}
		if(*tempheader=='D')
		{
			RposPID.Kd=FloatNum_analysis(tempheader+sizeof(uint8_t),';',&tempheader);
		}
	}
	else if(DBG_RxProBuf[3]=='T'&&DBG_RxProBuf[4]=='L')
	{
		uint8_t* tempheader = &DBG_RxProBuf[5];
		if(*tempheader=='P')
		{
			TlinePID.Kp=FloatNum_analysis(tempheader+sizeof(uint8_t),',',&tempheader);
		}
		if(*tempheader=='I')
		{
			TlinePID.Ki=FloatNum_analysis(tempheader+sizeof(uint8_t),',',&tempheader);
		}
		if(*tempheader=='D')
		{
			TlinePID.Kd=FloatNum_analysis(tempheader+sizeof(uint8_t),';',&tempheader);
		}
	}

}

void DBG_DAS_Process(void)
{
	//----------------------速度
	//左轮目标速度设置
	if(DBG_RxProBuf[3]=='L'&&DBG_RxProBuf[4]=='V'&&DBG_RxProBuf[5]=='T')
	{
		uint8_t* tempheader = &DBG_RxProBuf[5];
		LeftMT.PVtarget=(int)FloatNum_analysis(tempheader+sizeof(uint8_t),';',&tempheader);
	}
	//右轮目标速度设置
	else if(DBG_RxProBuf[3]=='R'&&DBG_RxProBuf[4]=='V'&&DBG_RxProBuf[5]=='T')
	{
		uint8_t* tempheader = &DBG_RxProBuf[5];
		RightMT.PVtarget=(int)FloatNum_analysis(tempheader+sizeof(uint8_t),';',&tempheader);
	}
	//双轮目标速度设置
	else if(DBG_RxProBuf[3]=='B'&&DBG_RxProBuf[4]=='V'&&DBG_RxProBuf[5]=='T')
	{
		uint8_t* tempheader = &DBG_RxProBuf[5];
		LeftMT.PVtarget=RightMT.PVtarget=(int)FloatNum_analysis(tempheader+sizeof(uint8_t),';',&tempheader);
	}

	//----------------------路程
	//左轮目标路程设置
	else if(DBG_RxProBuf[3]=='L'&&DBG_RxProBuf[4]=='P'&&DBG_RxProBuf[5]=='T')
	{
		uint8_t* tempheader = &DBG_RxProBuf[5];
		LeftMT.PPtarget=(int)FloatNum_analysis(tempheader+sizeof(uint8_t),';',&tempheader);
	}
	//右轮目标路程设置
	else if(DBG_RxProBuf[3]=='R'&&DBG_RxProBuf[4]=='P'&&DBG_RxProBuf[5]=='T')
	{
		uint8_t* tempheader = &DBG_RxProBuf[5];
		RightMT.PPtarget=(int)FloatNum_analysis(tempheader+sizeof(uint8_t),';',&tempheader);
	}
	//双轮目标路程设置
	else if(DBG_RxProBuf[3]=='B'&&DBG_RxProBuf[4]=='P'&&DBG_RxProBuf[5]=='T')
	{
		uint8_t* tempheader = &DBG_RxProBuf[5];
		LeftMT.PPtarget=RightMT.PPtarget=(int)FloatNum_analysis(tempheader+sizeof(uint8_t),';',&tempheader);
	}
	//路程清零
	else if(DBG_RxProBuf[3]=='B'&&DBG_RxProBuf[4]=='P'&&DBG_RxProBuf[5]=='C')
	{
		clear_postition();
	}

	//----------------------控制模式标志位
	//控制模式标志位
	else if(DBG_RxProBuf[3]=='L'&&DBG_RxProBuf[4]=='C'&&DBG_RxProBuf[5]=='F')
	{
		uint8_t* tempheader = &DBG_RxProBuf[5];
		RightMT.flag=LeftMT.flag=(uint16_t)Numerical_analysis(tempheader+sizeof(uint8_t),';',&tempheader);
	}

	//----------------------车身物理参数设置
	//轮胎半径
	else if(DBG_RxProBuf[3]=='W'&&DBG_RxProBuf[4]=='L'&&DBG_RxProBuf[5]=='R')
	{
		uint8_t* tempheader = &DBG_RxProBuf[5];
		PhyParam.wheelR = FloatNum_analysis(tempheader+sizeof(uint8_t),';',&tempheader);
	}
	//轮胎周长
	else if(DBG_RxProBuf[3]=='W'&&DBG_RxProBuf[4]=='L'&&DBG_RxProBuf[5]=='C')
	{
		uint8_t* tempheader = &DBG_RxProBuf[5];
		PhyParam.wheelC = FloatNum_analysis(tempheader+sizeof(uint8_t),';',&tempheader);
	}
	//每度的脉冲数
	else if(DBG_RxProBuf[3]=='P'&&DBG_RxProBuf[4]=='P'&&DBG_RxProBuf[5]=='A')
	{
		uint8_t* tempheader = &DBG_RxProBuf[5];
		PhyParam.PulsePerAngle =Numerical_analysis(tempheader+sizeof(uint8_t),';',&tempheader);
	}
	//l方向反转
	else if(DBG_RxProBuf[3]=='L'&&DBG_RxProBuf[4]=='D'&&DBG_RxProBuf[5]=='T')
	{
		uint8_t* tempheader = &DBG_RxProBuf[5];
		PhyParam.lDirTurn =(Numerical_analysis(tempheader+sizeof(uint8_t),';',&tempheader)!=0);
	}
	//r方向反转
	else if(DBG_RxProBuf[3]=='R'&&DBG_RxProBuf[4]=='D'&&DBG_RxProBuf[5]=='T')
	{
		uint8_t* tempheader = &DBG_RxProBuf[5];
		PhyParam.rDirTurn =(Numerical_analysis(tempheader+sizeof(uint8_t),';',&tempheader)!=0);
	}

	//----------------------MCU软件参数设置
	//pwm最大值
	else if(DBG_RxProBuf[3]=='P'&&DBG_RxProBuf[4]=='W'&&DBG_RxProBuf[5]=='M')
	{
		uint8_t* tempheader = &DBG_RxProBuf[5];
		MCUParam.pwmvalmax = Numerical_analysis(tempheader+sizeof(uint8_t),';',&tempheader);
	}
	//pwm最大保护值
	else if(DBG_RxProBuf[3]=='P'&&DBG_RxProBuf[4]=='W'&&DBG_RxProBuf[5]=='P')
	{
		uint8_t* tempheader = &DBG_RxProBuf[5];
		MCUParam.pwmvalprt = Numerical_analysis(tempheader+sizeof(uint8_t),';',&tempheader);
	}

	//----------------------紧急停车和继续
	else if(DBG_RxProBuf[3]=='S'&&DBG_RxProBuf[4]=='T'&&DBG_RxProBuf[5]=='P')
	{
		PWM_STOP();
	}
	else if(DBG_RxProBuf[3]=='S'&&DBG_RxProBuf[4]=='T'&&DBG_RxProBuf[5]=='R')
	{
		PWM_START();
	}

}

void DBG_DFM_Process(void)
{
	double v;
	int o;
	long a;
	uint8_t* tempheader = &DBG_RxProBuf[3];
	if(*tempheader=='V')
	{
		v=FloatNum_analysis(tempheader+sizeof(uint8_t),',',&tempheader);
	}
	if(*tempheader=='O')
	{
		o=(int)FloatNum_analysis(tempheader+sizeof(uint8_t),',',&tempheader);
	}
	if(*tempheader=='A')
	{
		a=(long)FloatNum_analysis(tempheader+sizeof(uint8_t),';',&tempheader);
	}
	modelctrl(v, o,a,0);
}

void debugcmd_process(void)
{
	if(DBG_RxProBuf[0]=='P'&&DBG_RxProBuf[1]=='I'&&DBG_RxProBuf[2]=='D')//判断为PID命令，调用PID命令解析处理函数
	{
		DBG_PID_Process();
	}
	else if(DBG_RxProBuf[0]=='D'&&DBG_RxProBuf[1]=='A'&&DBG_RxProBuf[2]=='S')//判断为DAS命令，调用DAS命令解析处理函数
	{
		DBG_DAS_Process();
	}
	else if(DBG_RxProBuf[0]=='D'&&DBG_RxProBuf[1]=='F'&&DBG_RxProBuf[2]=='M')//判断为DFM命令，调用DFM命令解析处理函数
	{
		DBG_DFM_Process();
	}
	else
	{
		debugUartSend(DBG_RxProBuf, sizeof(DBG_RxProBuf)-1);
	}
}

void CTL_PID_Process(void)
{
	//左轮速度PID（增量）
	if(CTL_RxProBuf[3]=='L'&&CTL_RxProBuf[4]=='V')
	{
		uint8_t* tempheader = &CTL_RxProBuf[5];
		if(*tempheader=='P')
		{
			LspdPID.Kp=FloatNum_analysis(tempheader+sizeof(uint8_t),',',&tempheader);
		}
		if(*tempheader=='I')
		{
			LspdPID.Ki=FloatNum_analysis(tempheader+sizeof(uint8_t),',',&tempheader);
		}
		if(*tempheader=='D')
		{
			LspdPID.Kd=FloatNum_analysis(tempheader+sizeof(uint8_t),';',&tempheader);
		}
	}
	//右轮速度PID（增量）
	else if(CTL_RxProBuf[3]=='R'&&CTL_RxProBuf[4]=='V')
	{
		uint8_t* tempheader = &CTL_RxProBuf[5];
		if(*tempheader=='P')
		{
			RspdPID.Kp=FloatNum_analysis(tempheader+sizeof(uint8_t),',',&tempheader);
		}
		if(*tempheader=='I')
		{
			RspdPID.Ki=FloatNum_analysis(tempheader+sizeof(uint8_t),',',&tempheader);
		}
		if(*tempheader=='D')
		{
			RspdPID.Kd=FloatNum_analysis(tempheader+sizeof(uint8_t),';',&tempheader);
		}
	}
	else if(CTL_RxProBuf[3]=='L'&&CTL_RxProBuf[4]=='P')
	{
		uint8_t* tempheader = &CTL_RxProBuf[5];
		if(*tempheader=='P')
		{
			LposPID.Kp=FloatNum_analysis(tempheader+sizeof(uint8_t),',',&tempheader);
		}
		if(*tempheader=='I')
		{
			LposPID.Ki=FloatNum_analysis(tempheader+sizeof(uint8_t),',',&tempheader);
		}
		if(*tempheader=='D')
		{
			LposPID.Kd=FloatNum_analysis(tempheader+sizeof(uint8_t),';',&tempheader);
		}
	}
	else if(CTL_RxProBuf[3]=='R'&&CTL_RxProBuf[4]=='P')
	{
		uint8_t* tempheader = &CTL_RxProBuf[5];
		if(*tempheader=='P')
		{
			RposPID.Kp=FloatNum_analysis(tempheader+sizeof(uint8_t),',',&tempheader);
		}
		if(*tempheader=='I')
		{
			RposPID.Ki=FloatNum_analysis(tempheader+sizeof(uint8_t),',',&tempheader);
		}
		if(*tempheader=='D')
		{
			RposPID.Kd=FloatNum_analysis(tempheader+sizeof(uint8_t),';',&tempheader);
		}
	}
	else if(CTL_RxProBuf[3]=='T'&&CTL_RxProBuf[4]=='L')
	{
		uint8_t* tempheader = &CTL_RxProBuf[5];
		if(*tempheader=='P')
		{
			TlinePID.Kp=FloatNum_analysis(tempheader+sizeof(uint8_t),',',&tempheader);
		}
		if(*tempheader=='I')
		{
			TlinePID.Ki=FloatNum_analysis(tempheader+sizeof(uint8_t),',',&tempheader);
		}
		if(*tempheader=='D')
		{
			TlinePID.Kd=FloatNum_analysis(tempheader+sizeof(uint8_t),';',&tempheader);
		}
	}

}

void CTL_DAS_Process(void)
{
	//----------------------速度
	//左轮目标速度设置
	if(CTL_RxProBuf[3]=='L'&&CTL_RxProBuf[4]=='V'&&CTL_RxProBuf[5]=='T')
	{
		uint8_t* tempheader = &CTL_RxProBuf[5];
		LeftMT.PVtarget=(int)FloatNum_analysis(tempheader+sizeof(uint8_t),';',&tempheader);
	}
	//右轮目标速度设置
	else if(CTL_RxProBuf[3]=='R'&&CTL_RxProBuf[4]=='V'&&CTL_RxProBuf[5]=='T')
	{
		uint8_t* tempheader = &CTL_RxProBuf[5];
		RightMT.PVtarget=(int)FloatNum_analysis(tempheader+sizeof(uint8_t),';',&tempheader);
	}
	//双轮目标速度设置
	else if(CTL_RxProBuf[3]=='B'&&CTL_RxProBuf[4]=='V'&&CTL_RxProBuf[5]=='T')
	{
		uint8_t* tempheader = &CTL_RxProBuf[5];
		LeftMT.PVtarget=RightMT.PVtarget=(int)FloatNum_analysis(tempheader+sizeof(uint8_t),';',&tempheader);
	}

	//----------------------路程
	//左轮目标路程设置
	else if(CTL_RxProBuf[3]=='L'&&CTL_RxProBuf[4]=='P'&&CTL_RxProBuf[5]=='T')
	{
		uint8_t* tempheader = &CTL_RxProBuf[5];
		LeftMT.PPtarget=(int)FloatNum_analysis(tempheader+sizeof(uint8_t),';',&tempheader);
	}
	//右轮目标路程设置
	else if(CTL_RxProBuf[3]=='R'&&CTL_RxProBuf[4]=='P'&&CTL_RxProBuf[5]=='T')
	{
		uint8_t* tempheader = &CTL_RxProBuf[5];
		RightMT.PPtarget=(int)FloatNum_analysis(tempheader+sizeof(uint8_t),';',&tempheader);
	}
	//双轮目标路程设置
	else if(CTL_RxProBuf[3]=='B'&&CTL_RxProBuf[4]=='P'&&CTL_RxProBuf[5]=='T')
	{
		uint8_t* tempheader = &CTL_RxProBuf[5];
		LeftMT.PPtarget=RightMT.PPtarget=(int)FloatNum_analysis(tempheader+sizeof(uint8_t),';',&tempheader);
	}
	//路程清零
	else if(CTL_RxProBuf[3]=='B'&&CTL_RxProBuf[4]=='P'&&CTL_RxProBuf[5]=='C')
	{
		clear_postition();
	}

	//----------------------控制模式标志位
	//控制模式标志位
	else if(CTL_RxProBuf[3]=='L'&&CTL_RxProBuf[4]=='C'&&CTL_RxProBuf[5]=='F')
	{
		uint8_t* tempheader = &CTL_RxProBuf[5];
		SET_CTRLFLAG((CtrlFlagDef)Numerical_analysis(tempheader+sizeof(uint8_t),';',&tempheader));
	}

	//----------------------车身物理参数设置
	//轮胎半径
	else if(CTL_RxProBuf[3]=='W'&&CTL_RxProBuf[4]=='L'&&CTL_RxProBuf[5]=='R')
	{
		uint8_t* tempheader = &CTL_RxProBuf[5];
		PhyParam.wheelR = FloatNum_analysis(tempheader+sizeof(uint8_t),';',&tempheader);
	}
	//轮胎周长
	else if(CTL_RxProBuf[3]=='W'&&CTL_RxProBuf[4]=='L'&&CTL_RxProBuf[5]=='C')
	{
		uint8_t* tempheader = &CTL_RxProBuf[5];
		PhyParam.wheelC = FloatNum_analysis(tempheader+sizeof(uint8_t),';',&tempheader);
	}
	//每度的脉冲数
	else if(CTL_RxProBuf[3]=='P'&&CTL_RxProBuf[4]=='P'&&CTL_RxProBuf[5]=='A')
	{
		uint8_t* tempheader = &CTL_RxProBuf[5];
		PhyParam.PulsePerAngle =Numerical_analysis(tempheader+sizeof(uint8_t),';',&tempheader);
	}
	//l方向反转
	else if(CTL_RxProBuf[3]=='L'&&CTL_RxProBuf[4]=='D'&&CTL_RxProBuf[5]=='T')
	{
		uint8_t* tempheader = &CTL_RxProBuf[5];
		PhyParam.lDirTurn =(Numerical_analysis(tempheader+sizeof(uint8_t),';',&tempheader)!=0);
	}
	//r方向反转
	else if(CTL_RxProBuf[3]=='R'&&CTL_RxProBuf[4]=='D'&&CTL_RxProBuf[5]=='T')
	{
		uint8_t* tempheader = &CTL_RxProBuf[5];
		PhyParam.rDirTurn =(Numerical_analysis(tempheader+sizeof(uint8_t),';',&tempheader)!=0);
	}

	//----------------------MCU软件参数设置
	//pwm最大值
	else if(CTL_RxProBuf[3]=='P'&&CTL_RxProBuf[4]=='W'&&CTL_RxProBuf[5]=='M')
	{
		uint8_t* tempheader = &CTL_RxProBuf[5];
		MCUParam.pwmvalmax = Numerical_analysis(tempheader+sizeof(uint8_t),';',&tempheader);
	}
	//pwm最大保护值
	else if(CTL_RxProBuf[3]=='P'&&CTL_RxProBuf[4]=='W'&&CTL_RxProBuf[5]=='P')
	{
		uint8_t* tempheader = &CTL_RxProBuf[5];
		MCUParam.pwmvalprt = Numerical_analysis(tempheader+sizeof(uint8_t),';',&tempheader);
	}

	//----------------------紧急停车和继续
	else if(CTL_RxProBuf[3]=='S'&&CTL_RxProBuf[4]=='T'&&CTL_RxProBuf[5]=='P')
	{
		PWM_STOP();
	}
	else if(CTL_RxProBuf[3]=='S'&&CTL_RxProBuf[4]=='T'&&CTL_RxProBuf[5]=='R')
	{
		PWM_START();
	}

}

void CTL_DFM_Process(void)
{
	float v;
	float o;
	float a;
	uint8_t* tempheader = &CTL_RxProBuf[3];
	if(*tempheader=='V')
	{
		v=FloatNum_analysis(tempheader+sizeof(uint8_t),',',&tempheader);
	}
	if(*tempheader=='O')
	{
		o=FloatNum_analysis(tempheader+sizeof(uint8_t),',',&tempheader);
	}
	if(*tempheader=='D')
	{
		a=FloatNum_analysis(tempheader+sizeof(uint8_t),';',&tempheader);
	}
	modelctrl(v,o,a,0);
}

void CTL_DAQ_Process(void)
{
	char txt[50];
	BfloatDef bdata={0,0};
	//双轮真实速度
	if(CTL_RxProBuf[3]=='B'&&CTL_RxProBuf[4]=='V'&&CTL_RxProBuf[5]=='R')
	{
		bdata = get_speed(MPS);
		sprintf(txt,"RVRL%f,R%f;",bdata.leftval,bdata.rightval);
		ctrlUartSend((uint8_t*)txt);
	}
	//双轮当前路程
	else if(CTL_RxProBuf[3]=='B'&&CTL_RxProBuf[4]=='P'&&CTL_RxProBuf[5]=='R')
	{
		bdata = get_postition(MTR);
		sprintf(txt,"RPRL%f,R%f;",bdata.leftval,bdata.rightval);
		ctrlUartSend((uint8_t*)txt);
	}
	//双轮剩余路程
	else if(CTL_RxProBuf[3]=='B'&&CTL_RxProBuf[4]=='P'&&CTL_RxProBuf[5]=='D')
	{
		bdata = get_rest_postition(MTR);
		sprintf(txt,"RPDL%f,R%f;",bdata.leftval,bdata.rightval);
		ctrlUartSend((uint8_t*)txt);
	}
	//控制标志位
	else if(CTL_RxProBuf[3]=='B'&&CTL_RxProBuf[4]=='C'&&CTL_RxProBuf[5]=='F')
	{
		sprintf(txt,"RCF%d;",GET_CTRLFLAG());
		ctrlUartSend((uint8_t*)txt);
	}
}

void CTL_SPF_Process(void)
{
	float l,r;
	uint8_t* tempheader = &CTL_RxProBuf[3];
	if(*tempheader=='l')
	{
		r=FloatNum_analysis(tempheader+sizeof(uint8_t),',',&tempheader);
	}
	if(*tempheader=='r')
	{
		l=FloatNum_analysis(tempheader+sizeof(uint8_t),';',&tempheader);
	}
	spinfor(l,r,MPS);
}

void CTL_SPT_Process(void)
{
	float l,r,L,R;
	uint8_t* tempheader = &CTL_RxProBuf[3];
	if(*tempheader=='l')
	{
		r=FloatNum_analysis(tempheader+sizeof(uint8_t),',',&tempheader);
	}
	if(*tempheader=='r')
	{
		l=FloatNum_analysis(tempheader+sizeof(uint8_t),',',&tempheader);
	}
	if(*tempheader=='L')
	{
		L=FloatNum_analysis(tempheader+sizeof(uint8_t),',',&tempheader);
	}
	if(*tempheader=='R')
	{
		R=FloatNum_analysis(tempheader+sizeof(uint8_t),';',&tempheader);
	}
	spinto(l,r,MPS,L,R,MTR);
}
void ctrolcmd_process(void)
{
	if(CTL_RxProBuf[0]=='P'&&CTL_RxProBuf[1]=='I'&&CTL_RxProBuf[2]=='D')//判断为PID命令，调用PID命令解析处理函数
	{
		CTL_PID_Process();
	}
	else if(CTL_RxProBuf[0]=='D'&&CTL_RxProBuf[1]=='A'&&CTL_RxProBuf[2]=='S')//判断为DAS命令，调用DAS命令解析处理函数
	{
		CTL_DAS_Process();
	}
	else if(CTL_RxProBuf[0]=='D'&&CTL_RxProBuf[1]=='A'&&CTL_RxProBuf[2]=='Q')//判断为DAQ命令，调用DAQ命令解析处理函数
	{
		CTL_DAQ_Process();
	}
	else if(CTL_RxProBuf[0]=='D'&&CTL_RxProBuf[1]=='F'&&CTL_RxProBuf[2]=='M')//判断为DFM命令，调用DFM命令解析处理函数
	{
		CTL_DFM_Process();
	}
	else if(CTL_RxProBuf[0]=='S'&&CTL_RxProBuf[1]=='P'&&CTL_RxProBuf[2]=='F')//判断为SPF命令，调用SPF命令解析处理函数
	{
		CTL_SPF_Process();
	}
	else if(CTL_RxProBuf[0]=='S'&&CTL_RxProBuf[1]=='P'&&CTL_RxProBuf[2]=='T')//判断为SPT命令，调用SPT命令解析处理函数
	{
		CTL_SPT_Process();
	}
	else
	{
		ctrlUartSend((uint8_t*)"Err");
	}
}

//定义通道名帧头帧尾
uint8_t frameNameHead[] = "AABBCC";
uint8_t frameNameEnd[] = "CCBBAA";
//定义数据帧头帧尾
uint8_t frameDataHead[] = "DDEEFF";
uint8_t frameDataEnd[] = "FFEEDD";
//定义通道名
uint8_t name[] = {"lvr,lvt,rvr,rvt,lpr,lpt,rpr,rpt"};
//赋值数据
float channels[8];

void BLT_Send_WaveData(float ch0,float ch1,float ch2,float ch3,float ch4,float ch5,float ch6,float ch7)
{
    channels[0] = ch0;
    channels[1] = ch1;
    channels[2] = ch2;
    channels[3] = ch3;
    channels[4] = ch4;
    channels[5] = ch5;
    channels[6] = ch6;
    channels[7] = ch7;

    debugUartSend(frameNameHead,sizeof(frameNameHead)-1);
    debugUartSend(name,sizeof(name)-1);
    debugUartSend(frameNameEnd,sizeof(frameNameEnd)-1);
    debugUartSend(frameDataHead, sizeof(frameDataHead)-1);
    debugUartSend((uint8_t*)channels,sizeof(channels));
    debugUartSend(frameDataEnd,sizeof(frameDataEnd)-1);
}
